#pragma once

#include "auxiliary.h"
#include <vector>
using namespace std;
using namespace Eigen;
class Body
{
protected:
	int id = -1;                               // id 
	double tM = 0.;                            //total Mass
	double* pos = nullptr;
	double* vel = nullptr;
	double* acc = nullptr;
public:
	enum
	{
		RIGID = 0x00,
		BASE = 0x01,
	};
	static RCORDS m_s_rtype;                   //type of rotation coordinates.
	static unsigned int NC;                    //number of the absolute coordinates.
	static bool SetRcords(IN RCORDS rtype);
	Body(IN double m)noexcept;
	Body()noexcept;
	bool operator==(IN Body& other)const;
	bool operator<(IN Body& other)const;
	bool operator<=(IN Body& other)const;
	bool operator>(IN Body& other)const;
	bool A(OUT Matrix3d& M)const;
	bool SetID(IN int i);
	int GetID()const;
	virtual unsigned int Type()const = 0;
	virtual double TotalMass()const;
	virtual Vector3d Rho(const Vector3d& r)const;
	virtual Vector3d AngularVel()const;
	virtual ~Body();
	virtual unsigned int nMode()const;
	virtual bool Write(IN Json::Value& body)const;

public:
	friend class MBSystem;
	friend class JointBase;
};

class BaseBody :public Body
{
	VectorXd(*pFun)(double) = nullptr;
	VectorXd(*vFun)(double) = nullptr;
	VectorXd(*aFun)(double) = nullptr;
public:
	BaseBody();
	BaseBody(IN VectorXd(*p)(double),IN VectorXd(*v)(double),IN VectorXd(*a)(double));
	~BaseBody();
	unsigned int Type()const;
	VectorXd Velocity(IN double t);
	VectorXd Acceleration(IN double t);                     
	bool Update(IN double t);
	virtual bool Write(IN Json::Value& body)const;
};

class RigidBody :public Body
{
protected:
	Matrix3d M22;
	Matrix3d Jc;                                        //the inertia of moment of the centroids
protected:
public:
	RigidBody();
	RigidBody(IN double m);
	RigidBody(IN double m,IN Matrix3d& I);
	virtual unsigned int Type()const;
	virtual ~RigidBody();
	bool Calepsilon(OUT Vector3d& eps);
	Matrix3d& CalJc();

	virtual bool Write(OUT Json::Value& body)const;
};